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Gradient-like observer design on the Special Euclidean group SE(3) 
with system outputs on the real projective space 

Minh-Duc Hua, Tarek Hamel, Robert Mahony, Jochen Trumpf 


Abstract —A nonlinear observer on the Special Euclidean 
group SE(3) for full pose estimation, that takes the system out¬ 
puts on the real projective space directly as inputs, is proposed. 
The observer derivation is based on a recent advanced theory 
on nonlinear observer design. A key advantage with respect to 
existing pose observers on SE(3) is that we can now Incorporate 
in a unique observer different types of measurements such as 
vectorial measurements of known inertial vectors and position 
measurements of known feature points. The proposed observer 
is extended allowing for the compensation of unknown constant 
bias present in the velocity measurements. Rigorous stability 
analyses are equally provided. Excellent performance of the 
proposed observers are shown by means of simulations. 

I. Introduction 

The development of a robust and reliable estimator of 
the pose (i.e. position and attitude) of a rigid body is a 
key requirement for robust and high performance control 
of robotic vehicles. Pose estimation is a highly nonlinear 
problem in which the sensors normally utilized are prone 
to non-Gaussian noise [2]. Classical approaches for state 
estimation are based on nonlinear filtering techniques such 
as extended Kalman filters, unscented Kalman filters or 
particle filters. However, nonlinear observers have become 
an alternative to these classical techniques, starting with the 
work of Salcudean [23] for attitude estimation and subse¬ 
quent contributions over the last two decades [3], [7]-[12], 
[17], [19]-[22], [24]-[28]. Early nonlinear attitude observers 
have been developed on the basis of Lyapunov analysis. 
Recently, the attitude estimation problem has motivated the 
development of theories on invariant observers for systems 
endowed with symmetry properties [1], [5], [6], [15]-[18], 
[26]. For instance, complementary nonlinear attitude ob¬ 
servers exploiting the underlying Lie group structure of the 
Special Orthogonal group SO (3) are derived in [17] with 
proofs of almost global stability of the error system. A 
symmetry-preserving nonlinear observer design based on the 
Cartan moving-frame method is proposed in [5], [6], which 
is locally valid for arbitrary Lie groups. A gradient-like ob¬ 
server design technique for invariant systems on Lie groups 
is proposed in [16], leading to almost global convergence 
provided that a non-degenerate Morse-Bott cost function is 
used. More recently, an observer design method directly on 
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the homogeneous output space for the kinematics of me¬ 
chanical systems is proposed in [18], leading to autonomous 
error evolution and strong convergence properties. Finally, 
[15] extends the observer design methodology proposed in 
[18] in order to deal with the case where the measurement 
of system input is corrupted by an unknown constant bias. 

Full pose observer design, although less studied than 
attitude observer design, has recently attracted more atten¬ 
tion [2]-[4], [14], [21], [27], [28]. For instance, observers 
designed directly on SE(3) have been proposed using both 
full state feedback [4] or bearing measurements of known 
landmarks [3]. An observer on SO(3) x is proposed in 
[27], using full range and bearing measurements of known 
landmarks and achieving almost global asymptotic stability. 
In a prior work by the authors [14], a nonlinear observer 
on SE(3) is proposed using directly position measurements 
in the body-fixed frame of known inertial feature points or 
landmarks, with motivation strongly related to robotic vision 
applications using either stereo camera or Kinect sensor. The 
observer derivation is based on the gradient-like observer 
design technique proposed in [16], and the almost global 
asymptotic stability of the error system is proved by means 
of Lyapunov analysis. 

In this paper, we consider the question of deriving a 
nonlinear observer on SE(3) for full pose estimation that 
takes the system outputs on the real projective space RP^ 
directly as inputs. A key advance on our prior work [14] is 
the possibility of incorporating “naturally” in a sole observer 
both vectorial measurements (provided e.g. by magnetome¬ 
ters or inclinometers) and position measurements of known 
inertial feature points (provided e.g. by stereo camera). In 
addition, sharing the same robustness property with the ob¬ 
server proposed in [14], the algorithm here proposed is also 
well-posed even when there is insufficient data for full pose 
reconstruction using algebraic techniques. In such situations, 
the proposed observer continues to operate, incorporating 
what information is available and relying on propagation of 
prior estimates where necessary. Finally, as a complementary 
contribution, a modified version of the basic observer is 
proposed so as to deal with the case where bias is present 
in the velocity measurements. 

The remainder of this paper is organised as follows. 
Sectionllllformallv introduces the problem of pose estimation 
on SE(3) along with the notation used. In Section III, based 
on a recent advanced theory for nonlinear observer design 
directly on the output space [18], a nonlinear observer on 
SE(3) is proposed using direct body-fixed measurements of 
known inertial elements of the real projective space RP^ and 


the knowledge of the group velocity. Stability analysis is also 
provided in this section. Then, in Section |IV] the proposed 
basic observer is extended using Lyapunov theory in order 
to cope with the case where the measurement data of the 
group velocity are corrupted by an unknown constant bias. 
In Section |Vl the performance of the proposed observers 
are validated by means of simulation. Finally, concluding 
remarks are given in Section [VTl 

II. Preliminary material 


• For any a; € (or G RP^), the notation x G denotes 
the vector of first three components of x and the notation Xi 
stands for the i-th component of x. Thus, it can be written 

as a; = [x ^ 4 ]^. 

B. System equations and measurements 

The vehicle’s pose X G SE(3), defined by O, satisfies 
the kinematic equation 

X = F{X,A) ■= XA, (3) 


A. Notation 

Let {A} and {B} denote an inertial frame and a body- 
fixed frame attached to a vehicle moving in 3D-space, 
respectively. The vehicle’s position, expressed in the frame 
{,4}, is denoted as p G R^. The attitude of the vehicle is 
represented by a rotation matrix R G SO(3) of the frame 
{B} relative to the frame {A}. Let E G and G R^ 
denote the vehicle’s translational and angular velocities, both 
expressed in {B}. 

In this paper, we consider the problem of estimating the 
vehicle’s pose, which can be represented by an element of 
the Special Euclidean group SE(3) given by the matrix 

^■[01 

This representation, known as homogeneous coordinates, 
preserves the group structure of SE(3) with the GL(4) 
operation of matrix multiplication, i.e. X 1 X 2 G SE(3), 
\/Xi,X 2 G SE(3). Now let us recall some common defi¬ 
nitions and notation. 

• The Lie-algebra se(3) of the group SE(3) is defined as 

se(3) := I A G | 3 fl, E G R^ : A = 

with fix denoting the skew-symmetric matrix associated 
with the cross product by fl, i.e. flxf = x u, Vu G R^. The 
adjoint operator is a mapping Ad : SE(3) x se(3) —>■ se(3) 
defined as AdxA := XAX~^, with X G SE(3), A G se(3). 

• Eor any two matrices Mi,M 2 G R"^", the Euclidean 
matrix inner product and Erobenius norm are defined as 

(Ml, M 2 ) := tr(MTM2), ||Mi|| := iJ{Mi,Mi). 

Let Pa(M), VM G R"^", denote the anti-symmetric part of 
M, i.e. Pa(M) := (M - MT)/2. Let P : R^x^ se(3) 
denote the unique orthogonal projection of R'^x4 qjjjq se(3) 
with respect to the inner product (•, •), i.e. MA G sc(3), M G 
R'^x^^ one has 


■fix 1^11 

0 ojj’ 


gSE(3)cR^^^. (1) 


{A,M) = {A,V{M)) = {V{M),A). 


It is verified that for all Mi G R^^^, 7712,3 G "mi G 


P 




Pa(Mi) 7772 
0 0 


( 2 ) 


• Eor all X G SE(3), ^ 1,^2 G se(3), the following equation 
defines a right-invariant Riemannian metric (•) ■)x- 


{AiX,A 2 X)x '■= (^ 1 ,^ 2 )- 


with group velocity A G se(3). System Q is left invariant 
in the sense that it preserves the (Lie group) invariance 
properties with respect to constant translation and constant 
rotation of the body-fixed frame {B} X 1 — XqX. 

Assume that the group velocity A (i.e. and V) is 
bounded, continuous, and available to measurement. More¬ 
over, N G N’*' constant elements of the real projective space 
i/i G RP^ (7 = 1, • • • , N), known in the inertial frame {A}, 
are assumed to be measured in the body-fixed frame {B} as 

yi = h{X,yi) := ^_iJ'. G RP^, 7=1,---,A^. (4) 

'-y^\ 

Note that the Lie group action h : SE(3) x RP^ —>■ RP^ is a 
right group action in the sense that for all Xi,X 2 G SE(3) 
and y G RP^, one has h{X 2 ,h{Xi,y)) = h{XiX 2 ,y). Eor 
later use, define 

y ■= {yi,---,yN), Y ■■= {yi,---^m)- (5) 


Remark 1 Interestingly, by considering the measurement 
data in the real projective space RP^, we are able to combine 
in a sole pose obser\’er various types of measurements 
coming from sensors of different nature. For instance, from a 
stereo camera or a Kinect sensor we can obtain a matching 
of Ni G N"*" feature points whose position coordinates are 
known in both the inertial reference frame {A} and the 
current body-fixed frame {B}, i.e. one has 

p^ = R^{pi-p), i = l,---,Ni, 


with Pi, Pi G R^ the position coordinates of the feature points 
expressed in the frames {^} and {B}, respectively. Then, the 
following simple transformations: 


■= 

-'■ vWTT 


1 


■= Fi 

-*■ vWTT 


5 ViA • — 


5 ViA ' — 


•\/|PiP + 1 

1 


yield the following relations in the form ®.- 

Vi = ly-lM = * = 1 g--,2Gi, 

^yi\ 


with yi = yi^iY G RP^ and yi = [y_^ 1 / 1 , 4 ]^ G RP^. 

Such a transformation provides a “natural” scaling of the 
position measurements of known inertial feature points so 
that the measurement of a very far feature point will act 
closely to a vectorial measurement. On the other hand, 
assume also that the vehicle is equipped with N 2 G N'*" 





















vectorial sensors (e.g. magnetometer or inclinometer) so as 
to provide the measurements Vj € in the body-fixed 
frame {B} of N 2 Euclidean vectors (given for example by 
the geomagnetic field or the gravity field) whose coordinates 
Vj € in the inertial frame {A} are known. Then, one 
verifies that Vj = Vj and deduces the following relations 
in the form &: 


Note that (i = 1, • • • ,N) can be viewed as the estimates 
of Pi, since they converge to yi when E converges to I 4 ,. 
Note also that are computable by the observer. 

We now proceed the observer design. As proposed by [18], 
the observer takes the form 

X A(X,r)l, A(0)eSE(3), (8) 


Vi = I j = + 1, • • • , + iV2, 

1^ Vjl 

with yj = [y. 0]^ S yj = [y. 0]^ S y. := 

^ and 

anay. — |^.|. 


We verify that SE(3) is a symmetry group with group 
actions f : SE(3) x SE(3) —^ SE(3), ^ : SE(3) x se(3) —^ 
se(3) and p : SE(3) x RP^ —RP^ defined by 


f{Q,X) 

p{Q,y) 


XQ, 

Adq-iA = Q-^AQ, 

Q-A 

w^- 


Indeed, it is straightforward to verify that f, tp, and p are 
right group actions in the sense that (j}(Q 2 , 4 >{Qi,X)) = 
(j){QiQ 2 ,X), il}(Q 2 ,'f(Qi,A)) = f}(QiQ 2 ,A), and 

piQ 2 ,p{Qi,y)) = p{QiQ 2 ,y), for all Qi,Q 2 ,X e SE(3), 
A G se(3), and y G RP^. Clearly, one has 


p{Q,h{X,y,)) 


Q 

Q 


-1 x-Ai 

-1 x-^vi I 
\X-^yi\ I 


h{f{Q,X),y,), 


dcl)Q{X)[F{X,A)] = XAQ={XQ){Q-^AQ) 

= Fif(Q,X),fjiQ,A)). 

Thus, the kinematics Q are right equivariant in the sense 
of [18, Def. 2]. This is a condition allowing us to apply 
the theory proposed in [18] for nonlinear observer design 
directly on the output space. Note also that the system under 
consideration belongs to type I systems (see [18]) where both 
the velocity sensors and the state sensors are attached to the 
body-fixed frame. 


III. Gradient-like observer design 


Denote by X(t) G SE(3) the estimate of the pose X{t) 
and denote by R and p the estimates of R and p, respectively. 
R p 


One has X = 


^ ^ . Define the group error 

E{X,X) :=XX-^ G SE(3), 


(6) 


which is right invariant in the sense that for all X,X,Q G 
SE(3), one has E{XQ, XQ) = E(X,X). From now on, 
without confusion the shortened notation E is used for 
E(X,X). The group error E converges to the identity 
element I 4 G SE(3) iif X converges to X. For later use, 
define also the output errors Ci G RP^, with i = 1, ■ ■ ■ , N, 
as 




HX-\y,) 


Xy^ 

\Xy^\ 


Ejn 

\Eyi\' 


where A(2f,y) G se(3), which is a matrix-valued function 
of X and V with V defined by (|5]l, is the innovation term to 
be designed hereafter and must be right equivariant in the 
sense that VQ G SE(3): 

X(f(Q,X),p(Q,Y)) = X{X,Y), 

with p(Q,Y) := (p(Q, ),•••, p(Q, yAr)). Interestingly, 

if the innovation term A(A,y) is right equivariant, the 
dynamics of the group error E are autonomous [18, Th. 1]: 

E = -A{E,Y)E. (9) 

In order to determine the innovation term A(A,y), the 
following cost function is considered: 

C: SE(3) X (RP^ x • • • x RP^) —> R+, 

(A,F) 

i=l 

with positive constant parameters ki. It is easily verified that 
the cost function C{X, Y) is right invariant in the sense that 
C{<l)(Q,X),p{Q,Y)) = C{X,Y) for all Q G SE(3). From 
here, the innovation term A{X,Y) is computed as [18, Eq. 
(40)]: 

A(A,r) := (gradiC(A,y))A-\ (11) 

where grad^ is the gradient in the first variable, using a 
right-invariant Riemannian metric on SE(3). 


Xyi 


\Xy^ 


( 10 ) 


Lemma 1 The innovation term A(X,Y) defined by (II 11 1 is 
right equivariant and explicitly given by 

/ N \ 


A{X,Y) = -P[y2h{h-e,eJ)y^eJ , ( 12 ) 




with Ci considered as functions of X and yt, i.e. Ci = 


Xyi 

\Xy.\- 


Proof: The proof for A{X,Y) given by (HaIi to be right 
equivariant is straightforward. Now, using standard rules for 
transformations of Riemannian gradients and the fact that the 
Riemannian metric is right invariant, one obtains 

V 4 C(X,Y)[UX] = {gYad 4 C(X,Y),UX)x 

= {gTeid4C(X,Y)X-^X,UX)x 

= {grad^CiX,Y)X-\U) 

= {A(X,Y),U), 


(7) 
















with some U € se(3). On the other hand, using ( fTOb one 
deduces 


ViC{X,Y)[UX] = diC{X,Y)[UX] 








h- 


{XyMXy^)^ 

\Xyi\^ 


= - y^Vih - eieJ)Uei 

= tr (E*^i hih - eiej){ei - yi)ej 
= (-Efci Hh - eieJ)yieJ,U^ 


(UX)yi 

\xyi\ 


= {-'P(j2f=ikiih-eiej)yiejy Uy 

(14) 

Finally, the expression of A(X,y) given by (fTSl i is directly 
obtained from ( fOl l and (fl4l i. ■ 


Using the definition (|2]i of the projection P( ), the innova¬ 
tion term A(X, F) given by (fT 2 l i can be rewritten in matrix 
form as follows: 


A(X,F) 


X y.)x '^he^,4{ie^'^yi)e,-y^ 


N 


N 


(15) 


Using (|9]l, (fTTl i and (fT 2 l) . one deduces the error system 

^=-gradiC(£;,y) 

/ ^ 

= P ( ^ fci (/4 - eiCi^) yiCi^ 

\i=l 

with Ci considered as functions of E and yi, i.e. Cj = |^|*| . 

For the sake of analysis purposes, the following assump¬ 
tion is introduced. 



Assumption 1 (Observability) The set {yi € KP^,z = 
1, • • • ,X} satisfies one of the three following cases: 

m Case 1 (at least 2 vectorial and 1 position measure¬ 
ments): There exist two different points yt-^ and yi^ with 
iliiA = Vi^A = 0 and one point yj-^ such that yj^A H- 

• Case 2 (at least 1 vectorial and 2 position measure¬ 

ments): There exist one point yi-^ with yt-^A ~ 0 
two different points yj^ and yj^ (i.e., f yj^) with 
yjiA ^ ® VnA ^ Furthermore, the vector y. 
and the resultant vector Vj.^^ 'll 32 A V ■ ~ 'UjiA 11 

non-collinear. 

• Case 3 (at least 3 position measurements): There exist 
three different points yj^, yj^ and yj^ such that yj.t^A 

0, ijj^A 7^ 0 yj^A 7 ^ Furthermore, the resultant 
vectors := ^322 ■= h^AlLj^ “ 

ihAlLj^ and := yhAdj^ “ VhAy^^ are not all 
collinear. 

From here, the first result of this paper is stated. 

Theorem 1 Consider the kinematics OJ. Consider the ob¬ 
server ([8]l with the innovation term A{X,Y) given by (112b . 


Assume that Assumption Q] is satisfied. Then, the equilibrium 
E = li of the error system (Hsii is locally asymptotically 
stable. 


Proof: Since the right-hand side of (fTbl l is a gradient 
flow of C, in order to prove the local asymptotic stability of 
E = li, it suffices to prove that C{E,Y) is minimal when 
E = 14 ^. Note that 


C{E,Y) 


V{E) := 





Eyi 

\Eyi\ 


- -yi 


2 


(17) 


Let us prove that the function V{E) has a unique global 
minimum at E = I 4 , i.e. V{E) = 0 AA E = I 4 . 

First, it is straightforward to verify that V(/ 4 ) = 0. Denote 



Pe 

1 


, with Rf, € SO(3),Pe G Now assuming 


that V(E) = 0, we only have to prove that E = I 4 or, 
equivalently. Re = I 3 and pe = 0 . In view of (fTTl i and 
V{E) = 0, one deduces that Eyi = \Eyi\yi, Vi, i.e. 


{ Rey. + PeViA = \J'yl4 + \Rey,+ Pe’yiA? y, (i8a) 
'PiA = tJ'yU + IRey^+Pe-piAl^ 'y^A (i8b) 

Let us consider all the three cases of Assumption [T] 

• Case 1 of Assumption m Since = ^^ 2,4 = 0, one 
has \y. I = \y. \ = 1. Then, one deduces from (IISab that 
i?eV = y. and Refj. = y. ■ These equalities and the 

^ ^'Z '2 ^7.2 

non-collinearity of y. and y. allows one to deduce that 
Re = I 3 . Since yj^A 7 ^ 0, (IISm implies that \Eyj^ \ = 1. As 
a consequence, one deduces from (IlSab that pe = 0 . 

• Case 2 of Assumption [ 7 } Analogously to case 1, one 
deduces that Refj. = y. . Now, since yj^^A 7^ 0 and 
Vj 2.4 7^ 0’ ( IlSbb implies that \Eyjf = \Eyjf = 1. Then, 
from (IlSab one obtains 

j{Re-l3)y.^ +PeypA = ^ 
y{Re-l3)y.^ +Pey32A = 0 

From here, simple combination yields ReVp^ = '*^ 312 ^ with 
defined in Assumption [T] It is easily verified that 
UJ 12 0 using the fact that yj^ and yj^ are non-collinear by 
assumption. Furthermore, since y. and are non-collinear 
by assumption, relations iieV- = Vj and ReVj^^ = 
obtained previously imply that Re = I 3 . From here, it is 
straightforward to deduce that pe = 0 . 

• Case 3 of Assumption [ 7 } Analogously to case 2, one 
deduces from (fTsT i that \Eyp \ = \Eyj.^ \ = \Eyjf = 1 and 

{ {Re - l3)y.^ EPe'ynA = 0 
{Re - l3)y.^ FPe'ynA = 0 
{Re - l3)y.^ FPey^A = 0 

From here, analogously to case 2 one deduces that ReVj^^ = 
Vj, 2 , ReVj^^ = Vj^^, ReVj^, = and that and 

are not null. Then, using the non-collinearity assumption 
of the vectors and it is easily deduced that 

Re = I 3 and, consequently, that pe = 0. ■ 



















IV. Observer design with velocity bias 

COMPENSATION 

The observer developed in the previous section will be ex¬ 
tended in order to cope with the case where the measurement 
Ay G se(3) of the group velocity A G se(3) is corrupted by 
an unknown constant bias 6 ^ G se(3), i.e. Ay = A + bA- 

Assumption 2 Assume that the following matrices G G 
and H G are full rank: 

2=1 


^2 = 1 
N 


k2=l 


- - yjf) 

2=1 

The condition on the set {yi G 


= l,--- ,N} 

evoked in Assumption [1] ensures that it is always possible 
to choose a set of parameters {ki,i = 1, • • • , N} such that 
G and H are full rank (i.e. invertible). Now, the second result 
of this paper is stated. 

Proposition 1 Consider the observer system 
'jc = X{Ay-bA)-A{X,Y)X 




Sa = -hP 


{h-e.eJ)y.eJX-^ 



1 


Vi,{E,bA)-.= -J2k 


2=1 


Eyi 


\Eyi 


yi 


1 

2 kb 


IlfcAf. (23) 


Analogously to the proof of Theorem [T] it can be verified 
that Vb{E,bA) is locally positive-definite and has a unique 
global minimum at (E, Ida) = {I 4 , 0), i.e. Vb{E, Ida) = 0 
iE,bA) = {h,0). 

The time-derivative of Vb satisfies 

Vb = (^- YlfLi h [li-eiCi^) yieJ,Ady^bA - A{E,Y) 

(bA,bA 

= - p (E*=i (-^4 - e^ej) yicj ) 

= -||A(A,y)f. 


\ 2 = 1 

[A(0)gSE(3), bA{0)Gsc{3) 

with A(A, Y) given by (112b . Assume that Assumptions\I\and 
^are satisfied. Assume also that A and X are bounded for all 
time. Then, the equilibrium (E, bA) = (fr, 0) of the dynamics 
of {E, bA), with bA ■= bA — bA, is locally asymptotically 
stable. 

Proof: It is easily verified that bA = —Ida and X = 
X{A + Ida) — A(A, Y)X. Then, one deduces 

E = [Ady^bA - A{E, Y)^ E. (22) 

Consider the candidate Lyapunov function 

N 2 


Since the dynamics of {E, Ida) are not autonomous, LaSalle’s 
theorem does not apply to deduce the convergence of Vt to 
zero. Thus, the next step of the proof consists in proving that 
Vb is (locally) uniformly continuous in order to deduce, by 
application of Barbalat’s lemma, the convergence of Vb to 
zero. To this purpose it suffices to prove that Vb is bounded. 
In view of (l24l) . Vb is bounded if {i = I,-- - ,N) are 
bounded, where (using (| 2 ^ and the relation = |^^*| ) 

G = {Ia - eieJ){Ady^bA - A{E,Y))ei. 

According to Assumption |2] there exists at least one point yi 
such that its fourth component 4 is not null. This indicates 
that for a given small number e > 0 there exists be > 0 such 
that if |pe| > be or \bA\ > be then Vb{E, Ida) > £■ Therefore, 
there exists a small enough neighborhood iBe G SE(3) x 
of the point (/ 4 , 0 ) such that if (i5(0), 6^(0)) G Se then 
Vb(E(0), bA(0)) < £. Since Vb(E, bA) is non-increasing, one 
has Vb{E{f),bA{t)) < e,Vf < 0. This implies that E and 
bA remain bounded. Since X is bounded by assumption, 
one deduces from the boundedness of E that X is also 
bounded, which in turn implies the boundedness of E and 
G- This concludes the proof of (local) uniform continuity of 
Vb and the convergence of Vb to zero. One easily verifies 
that {E, bA) = {I 4 , 0) is an equilibrium of the error system. 
Let us prove the local stability of this equilibrium. To this 
purpose let us first prove that V(i?, 6 ^) G 

Vb(E,bA)=0 if E = l4 
VbiE,bA)<0 if E^h 

Consider a first order approximation of E = 
around I 4 as 

' Pe = £ 

Re = 


Re 

0 


Pe 

1 


I 3 + £r 


with EpjEr G We only need to prove that 

Vb(E, Ida) = 0 4^ Sp = £r = 0. 

Note that (l24b and (fTsT i indicate that the relation = 0 is 
equivalent to 

[ EZi xy^ =0 

\ EZlk^i{eJy^)ei-y.)ei^4 = 0 

In first order approximations, one verifies that 

■y. + Srxy, + yi,4£p 


(25) 


Eyi = 

and, thus, 

e, = 


—2 


\Ey^\ = 1 




Ejn 

\Eyi\ 


y, + + y^, 4 .{h - y,k )£; 


2/i.4 


-T° 


Therefore, in first order approximations the equalities in (l25l l 
can be rewritten as 


! ktm,4y. 


Sr) - 


MW 


(26a) 




( 24 ) 


















Since G is full rank according to Assumption|2] it is deduced 
from (I26al i that Sr = G~^ Sp. This relation 

along with i26H yields Hsp = 0. Since H is also full 
rank by Assumption |2] it is deduced that Ep = 0 and, 
consequently, e^. = 0. 

It remains to prove to convergence of to zero. From 
the convergence of E to G (proven previously) and (l22l i. the 
application of Barbalat’s lemma yields the convergence of E 
to zero. Finally, Eq. (l22l i and the convergence of E and of 
A{E,Y) to zero imply the convergence of Ba to zero. ■ 
The estimate Ba plays the role of integral correction for 
the error dynamics (l22l i. allowing for the compensation of the 
unknown constant bias Ba- It may, however, grow arbitrarily 
large, resulting in slow convergence and sluggish dynamics 
of the error evolution. This leads us to replace hereafter the 
integral term Ba, with dynamics given by (I21bl) . by an “anti¬ 
windup” integrator similar to the one proposed in [11], [13 . 

More precisely, by decomposing Ba as Ba = 

with Bq/Bv G one rewrites the dynamics (I21bb of the 
estimated bias Ba as 

Bq = kbR^(^A + X p 

by = kbR^ Va 

with Fa := Y.f=ikieiA{{eJyi)ei - y^) and ^a ■= 
“iSfci ki§.i X y^. From here, the following modified dy¬ 
namics of Ba (i.e. of Bq and by) are proposed; 

I So =kbR'^{nA+^VAX p) - Knibn - sat5„(So)) (28a) 

I^Sy = kbFtJVA — Ky(by — sat^^,(S\/)) (28b) 

with initial conditions |6o(0)| < 6 n and |6\/(0)| < Sy; 
KQ and Ky two positive numbers; Sq and Sy two positive 
parameters associated with the classical functions sat 5 jj(-) 
and sat 5 jj(-) defined by sat 5 (a;) = a;min(l, 5/\x\),'ix £ 

Corollary 1 Consider the observer ( I21al i-K l281 l. Assume that 
Sq and Sy are chosen such that |6o| < Sq and 16^1 < 
Sy. Then, provided that all the assumptions of Proposition 
| 7 ] are satisfied, the local asymptotic stability property of 
Proposition Q] holds. 

Proof: Based on the inequality \b—sats{b—b)\ < |5| for 
all b and provided that S > |6| (see e.g. [11]), it can be 
easily proved that the time-derivative of Vb defined by (I23I) 
satisfies Vb < ^)IP- Therefore, the local asymptotic 

stability property given in Proposition [T] still holds when the 
dynamics of Ba given by (I21bb is replaced by (l28T l. ■ 

V. Simulation results 

In this section, the performance of observer (12 1 ab - Kl28T l is 
illustrated by simulations. The angular and translational ve¬ 
locity measurements are corrupted by the following constant 
biases; 

bQ = [-0.02 0.02 0.01]^ (rad/s), 

hy = [Q.2 -0.1 0.1]^ (m/s). 


(&n)x by 
0 0 


We consider the three following cases where only three 
system outputs yi £ RP^ of known inertial elements yi £ 
RP^ (i = 1,2,3) are available to measurement; 

• Case 1; corresponds to Case 1 of Assumption [T] in 

which two vectorial measurements vi,V 2 £ R^ and 
the position measurement pi £ R^ of one feature 
point are available, where vi = vi, V 2 = R^V 2 , 
Pi = R^ {pi — p), with ui = [0 0 1]"’^, V 2 = 
[v^/2 1/2 0]^ and pi = [1 0 0]^. 

• Case 2; corresponds to Case 2 of Assumption [T] in 

which one vectorial measurement vi £ R^ and the posi¬ 
tion measurements pi,p 2 £ R^ of two feature points are 
available, where vi = R^vi, pi = R^{pi — p), P 2 = 
R^[P 2 — p), with = [0 0 1]^, Pi = [1 0 0]^ 

and p 2 = [-1/2 VS/2 0]^. 

• Case 3; corresponds to Case 3 of Assumption [T] in 

which the position measurements pi,P 2 ,P 3 £ of 
three feature points are available, where pi = R^ {pi — 
p), P 2 = R^ {P 2 - p), P3 = R^{P3 - p), with 
Pi = [1 0 0]^, P 2 = [-1/2 v^/2 0]^ and pa = 

[-1/2 - V3/2 0]^. 

Recall that Remark [T] explains how to transform a vector or 
a position of a feature point into a corresponding element of 
RP^. 

The gains and parameters involved in the proposed ob¬ 
server are chosen as follows; 

ki = k2 = ks = 2 , kb = 1 , 

KQ = Ky = 10, Sa = 0.052, Sy = 0.346. 

For each simulation run, the proposed filter is initialized at 
the origin (i.e. R = I^^p = 0,6a = 0,= 0) while 
the true trajectories are initialized differently. Combined 
sinusoidal inputs are considered for both the angular and 
translational velocity inputs of the system kinematics. The 
rotation angle associated with the axis-angle representation 
is used to represent the attitude trajectory. One can observe 
from Figure [T| that the observer trajectories converge to the 
true trajectories after a short transition period for all the 
three considered cases. Figure |2] shows that the norms of 
the estimated velocity bias errors |6a| and |6y| converge to 
zero, which means that the group velocity bias Ba is also 
correctly estimated. 


VI. Conclusions 

In this paper, we propose a nonlinear observer on SE{3) 
for full pose estimation that takes the system outputs on the 
real projective space RP^ directly as inputs. The observer 
derivation is based on a recent observer design technique 
directly on the output space, proposed in [18]. An advantage 
with respect to our prior work [14] is that we can now 
incorporate in a unique observer different types of mea¬ 
surements such as vectorial measurements of known inertial 
vectors and position measurements of known feature points. 
The proposed observer is also extended on SE(3) x se(3) 
so as to compensate for unknown additive constant bias in 
the velocity measurements. Rigorous stability analyses are 









Fig. 1. The rotation angle and the position tracking performance of the 
proposed observer. Note that the dashed lines are the estimated trajectories 
(for Cases 1 (green), Case 2 (blue). Case 3 (red)) while the solid line (black) 
represents the true trajectory. 



t{s) 


Fig. 2. The norms of the estimated velocity bias errors |6n| and |6v| vs. 
time. 

equally provided. Excellent performance of the proposed 
observers are justified through simulations. 
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